//
//MT6816CT_STD.C
// Created by Xiaoyun on 23.3.25.
//
#include "mt6816ct-std.h"
#include <stdio.h>
#include <stdbool.h>


#define ROW_DATA (*(MT6816_REG_t*)(mt6816_spi_dma_buff+1))

/**
 * @brief struct for unpack data
 */
typedef struct {
  uint8_t angleh: 8;
  uint8_t pc: 1;
  uint8_t no_mag: 1;
  uint8_t anglel: 6;
  uint8_t : 3;
  uint8_t over_speed: 1;
  uint8_t : 4;
} MT6816_REG_t;

/**
 * @brief circular queue for MT6816 data
 */
typedef struct {
  uint8_t front, rear;
  uint8_t size;
  MT6816_Data_t data[MT6816_QueueSize];
} MT6816_CQueue;


static uint8_t mt6816_spi_dma_buff[4] = {0};
static uint8_t mt6816_spi_dma_trans[4] = {0x83, 00, 00, 00};//,0x80|0x04};
static MT6816_CQueue mt6816_data_queue = {0};

/*----------------Weak Functions(User to implement)----------------*/

/**
 *
 * @param pTxData : uint8_t*
 * @param pRxData : uint8_t*
 * @param size : data length of uint8_t
 */
__weak void MT6816_SPI_TransmitReceive(uint8_t *pTxData, uint8_t *pRxData, uint8_t size) {

}

/*----------------QUEUE_Functions----------------*/

/**
 *
 * @param Q : MT6816_CQueue
 * @return if the queue is empty
 */
static bool QueueEmpty(MT6816_CQueue Q) {
  if (Q.size == 0) return true;
  return false;
}

/**
 *
 * @param Q  : MT6816_CQueue
 * @return if the queue is full
 */
static bool QueueFull(MT6816_CQueue Q) {
  if (Q.size == MT6816_QueueSize) return true;
  return false;
}

/**
 *
 * @param Q : MT6816_CQueue*
 * @param x : MT6816_Data_t
 * @return if EnQueue succeed
 */
static bool EnQueue(MT6816_CQueue *Q, MT6816_Data_t x) {
  if (QueueFull(*Q)) return false;
  Q->data[Q->rear] = x;
  Q->rear = (Q->rear + 1) % MT6816_QueueSize;
  Q->size++;
  return true;
}

/**
 *
 * @param Q : MT6816_CQueue*
 * @param x : MT6816_Data_t* the data in front
 * @return if DnQueue succeed
 */
static bool DeQueue(MT6816_CQueue *Q, MT6816_Data_t *x) {
  if (QueueEmpty(*Q)) return false;
  *x = Q->data[Q->front];
  Q->front = (Q->front + 1) % MT6816_QueueSize;
  Q->size--;
  return true;
}

/*----------------MT6816_Functions----------------*/

/**
 * @brief Read data of MT6816 to queue, This function needs to be executed in each control cycle
 */
void MT6816_SPI_Read2Queue() {
  MT6816_SPI_TransmitReceive(mt6816_spi_dma_trans, mt6816_spi_dma_buff, sizeof(uint8_t) * 4);
//  uint16_t check_data = *(uint16_t*)(mt6816_spi_dma_buff+1);
//  uint8_t Check_Sum=0;
//  for(uint8_t i=0;i<16;i++){
//    Check_Sum+=(check_data&0x0001);
//  }
//  Check_Sum&=0x0001;
  MT6816_Data_t _;
  MT6816_Data_t data;
  data.angle = (ROW_DATA.angleh << 6) | ROW_DATA.anglel;
  data.no_mag = ROW_DATA.no_mag;
  data.over_speed = ROW_DATA.over_speed;
  if (QueueFull(mt6816_data_queue))DeQueue(&mt6816_data_queue, &_);
  EnQueue(&mt6816_data_queue, data);
}

/**
 * @brief Get the current data when seq equals 0, Get past data when seq is greater than 0
 * @param data : MT6816_Data_t* Data in queue
 * @param seq : uint8_t the data index seq
 * @return if queue is empty return false
 */
bool MT6816_Get_Data(MT6816_Data_t *data, uint8_t seq) {
  if (seq >= MT6816_QueueSize || (seq + 1) > mt6816_data_queue.size)return false;
  seq = (seq + mt6816_data_queue.front) % MT6816_QueueSize;
  *data = mt6816_data_queue.data[seq];
  return true;
}

/**
 *
 * @brief Get the amount of data in queue
 * @return amount of data
 */
uint8_t MT6816_Get_Queue_Size() {
  return mt6816_data_queue.size;
}

/**
 *
 * @brief Get the latest read data
 * @param data : MT6816_Data_t*
 * @return if queue is empty return false
 */
bool MT6816_Get_Queue_Head(MT6816_Data_t *data) {
  if (QueueEmpty(mt6816_data_queue)) {
    return false;
  }
  *data = mt6816_data_queue.data[mt6816_data_queue.front];
  return true;
}

